#include <highgui.h>
#include <iostream>
#include <cv.h>

#include "HandInterface.hpp"
using namespace cv;
using namespace std;
using namespace hi;

class LocalisationTester {
public:
	LocalisationTester();

	cv::Ptr< ResourceTracker > rt;

	cv::Ptr< HandLocaliser > hl;
	
	cv::Ptr< cv::VideoCapture > capPtr;
	cv::Ptr< VideoPointTagSequence > seqPtr;
	cv::Mat frame;
	cv::Rect trackingRect;
	int localisedHandInFrame;
	
	void test( const char* videoNameString );
private:
};

LocalisationTester::LocalisationTester() {
};

void LocalisationTester::test( const char* videoNameString ) { 

	std::cerr << videoNameString << ";" ;
	
	rt = new ResourceTracker( "../config/runtimeSettings/cfg.xml" );
	rt->loadCategory("TestData");
	
	capPtr = rt->getFileVideoCapture( videoNameString );
	seqPtr = rt->getPointTags( videoNameString );
	
	int tagCount = seqPtr->tagCount;
	
	if (tagCount < 1) {
		CV_Error( CV_StsError, "No tags!"); //throws exception!
	}
	
	hl = new HandLocaliser();
	
	int localisationframeNumber = 0;
	
	for ( ; localisationframeNumber < tagCount; localisationframeNumber++ ) {
		try {
			(*capPtr) >> frame;
			Point groundTruth = seqPtr->getNextPointTag();
			trackingRect = hl->localiseHand( frame );
			
			Point trackingRectCentre = Point( trackingRect.x + trackingRect.width/2, trackingRect.y + trackingRect.height/2 );
			
			int dX = groundTruth.x - trackingRectCentre.x;
			int dY = groundTruth.y - trackingRectCentre.y;
			
			double frameError = std::sqrt( dX*dX + dY*dY );
			
			std::cerr << localisationframeNumber + 1 << ";" ;
			std::cerr << frameError << std::endl ;
			
			break;
		}
		catch (cv::Exception e) {
			if ( e.code == HI_ERR_NOFACEFOUND ) {
				continue;
			} else {
				throw;
			}
		}
	}
}



int main( int argc, char** argv ) {
	
	for (int i = 1; i<argc; i++) {
		LocalisationTester lTester;
		lTester.test( argv[i] );
	}
	
}


